package algorithm;

import cn.hutool.core.util.ArrayUtil;
import com.alibaba.fastjson.JSONObject;
import org.apache.poi.ss.formula.functions.T;

import java.util.ArrayList;
import java.util.List;

/**
 * @author wangxiaohui
 * @date 2023/6/1 10:36
 */
public class PidAl {


    public static void main(String[] args) {
        List<Integer> aa = new ArrayList<>();
        aa.add(1);
        aa.add(4);
        aa.add(3);
        aa.add(8);
        aa.add(7);
        float VCM_MuBiao = 10;
        int MIE_Data = 4;
        while (true) {
            TIM tim = PidAl.TIM4_IRQHandler(ArrayUtil.toArray(aa, Integer.class), VCM_MuBiao, MIE_Data);
            System.out.println(JSONObject.toJSONString(tim));
        }
    }


    public static TIM TIM4_IRQHandler(Integer[] ADC_Value, float VCM_MuBiao, int MIE_Data) {
        /* USER CODE BEGIN TIM4_IRQn 0 */
        //0.1mS//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算//PID计算
        float VCM_A_PHASE = 0;
        float VCM_B_PHASE = 0;
        int i=10;
        int GuoLing1=0;
        int GuoLing2=0;

        float VCM_DangQian_0=0;       //当前值
        float Ek_0=0, Ek_1=0, Ek_2=0;   //偏差值

        float Kp=0, Ki=0, Kd=0;  //PID系数
        float PID_OUT=0, PID_OUT_P=0, PID_OUT_I=0, PID_OUT_D=0; //PID输出
        float PWM_VCM_A0=0, PWM_VCM_B0=0;     //PWM占空比参数缓冲
        float PWM_VCM_A =0, PWM_VCM_B =0;  //PWM占空比参数

        /* USER CODE END TIM4_IRQn 0 */
//        HAL_TIM_IRQHandler(&htim4);
        /* USER CODE BEGIN TIM4_IRQn 1 */
        VCM_DangQian_0=0; i=5;
        while(i-- > 0){
            VCM_DangQian_0+=ADC_Value[i];
        } //平均值
        float VCM_DangQian = VCM_DangQian_0 / 5;            //平均值

        Ek_2 = Ek_1;                      //上上次偏差
        Ek_1 = Ek_0;		                //上次偏差
        Ek_0 = VCM_MuBiao - VCM_DangQian;   //当前偏差

        switch(MIE_Data) {
            case 0://停止
                PWM_VCM_A0=1200*2;  //+↑；-↓
                PWM_VCM_B0=1200*2;  //+↑；-↓

                GuoLing1=0;
                GuoLing2=0;
                break;
            case 1://吸气
                PWM_VCM_A0=1200*2;//+↑；-↓

                Kp=10f;     //TiaoShi_P;           //PID分离
                Ki=0.023f;  //TiaoShi_I;           //PID分离
                Kd=800f;    //TiaoShi_D;

                PID_OUT_P=Kp*(Ek_0-Ek_1);             //增量PID计算
                PID_OUT_I=Ki*(Ek_0);                  //增量PID计算
                PID_OUT_D=Kd*(Ek_0-Ek_1-Ek_1+Ek_2);   //增量PID计算
                PID_OUT=PID_OUT_P+PID_OUT_I+PID_OUT_D;//增量PID计算

                if(Ek_0<10){
                    GuoLing1=1;
                }
                if(GuoLing1 > 0) {
                    PWM_VCM_B0 -= PID_OUT;
                }else{
                    PWM_VCM_B0 = 0;
                }

                break;
            case 2://呼气
                PWM_VCM_B0=1200*2;//+↑；-↓

                Kp=3.5f;     //TiaoShi_P;           //PID分离
                Ki=0.003f;  //TiaoShi_I;           //PID分离
                Kd=800f;     //TiaoShi_D;

                PID_OUT_P=Kp*(Ek_0-Ek_1);             //增量PID计算
                PID_OUT_I=Ki*(Ek_0);                  //增量PID计算
                PID_OUT_D=Kd*(Ek_0-Ek_1-Ek_1+Ek_2);   //增量PID计算
                PID_OUT=PID_OUT_P+PID_OUT_I+PID_OUT_D;//增量PID计算
                if(Ek_0>-350) {
                    GuoLing2=1;
                }
                if(GuoLing2 > 0) {
                    PWM_VCM_A0 += PID_OUT;
                }else{
                    PWM_VCM_A0=0;
                }

                break;
            default://无
                PWM_VCM_A0=0;
                PWM_VCM_B0=0;
                break;
        }

        if(PWM_VCM_A0<-9000||PWM_VCM_A0>9000) {
            if(PWM_VCM_A0>0) {
                PWM_VCM_A0=9000;
            }else{
                PWM_VCM_A0=-9000;
            }
        }
        if(PWM_VCM_A0>0) {
            VCM_A_PHASE=1;
            PWM_VCM_A=PWM_VCM_A0;
        } else {
            VCM_A_PHASE=0;
            PWM_VCM_A=-PWM_VCM_A0;
        }
        if(PWM_VCM_B0<-9000||PWM_VCM_B0>9000) {
            if(PWM_VCM_B0>0){
                PWM_VCM_B0=9000;
            }else{
                PWM_VCM_B0=-9000;
            }
        }
        if(PWM_VCM_B0>0){
            VCM_B_PHASE=1;
            PWM_VCM_B=PWM_VCM_B0;
        } else {
            VCM_B_PHASE=0;
            PWM_VCM_B=-PWM_VCM_B0;
        }
        TIM tim = new TIM();
        tim.setCCR1(PWM_VCM_B);
        tim.setCCR2(PWM_VCM_A);
        tim.setVCM_A_PHASE(VCM_A_PHASE);
        tim.setVCM_B_PHASE(VCM_B_PHASE);
        return tim;
        // 调试
//        TS_PWM_B=PWM_VCM_B0;
//        TS_PWM_A=PWM_VCM_A0;
//
//        TS_OUT=PID_OUT;
//        TS_OUT_P=PID_OUT_P;
//        TS_OUT_I=PID_OUT_I;
//        TS_OUT_D=PID_OUT_D;
        /* USER CODE END TIM4_IRQn 1 */
    }
}
